#!/usr/bin/env python3

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # Declare launch arguments
    use_sim_time = LaunchConfiguration('use_sim_time')
    namespace = LaunchConfiguration('namespace')
    
    # Launch arguments
    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true'
    )
    
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace'
    )
    
    # Yaw Controller Node (only)
    yaw_controller_node = Node(
        package='yaw_angle_controller',
        executable='yaw_controller',
        name='yaw_controller',
        namespace=namespace,
        output='screen',
        parameters=[
            {'use_sim_time': use_sim_time},
            # Use the config file from pb2025_nav_bringup
            os.path.join(
                get_package_share_directory('pb2025_nav_bringup'),
                'config', 'reality', 'nav2_params.yaml'
            )
        ],
        remappings=[
            ('cmd_vel', 'cmd_vel_yaw'),  # Separate topic to avoid conflicts
        ]
    )
    
    # Create the launch description and populate
    ld = LaunchDescription()
    
    # Add launch arguments
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_namespace_cmd)
    
    # Add node
    ld.add_action(yaw_controller_node)
    
    return ld